package com.exc4.jbv;

import lejos.nxt.ADSensorPort;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.SensorPort;

public class ThreeColorSensor {
	private LightSensor ls;
	private int blackLightValue;
	private int whiteLightValue;
	private int thirdColorValue;
	private int upperThreshold;
	private int lowerThreshold;
	private String thirdColorName;
	private ADSensorPort port;

	public ThreeColorSensor(SensorPort p, String thirdColor) {
		ls = new LightSensor(p);
		port = p;
		// Use the light sensor as a reflection sensor
		ls.setFloodlight(true);
		thirdColorName = thirdColor;
	}

	private int read(String color) {

		int lightValue = 0;

		while (Button.ENTER.isDown())
			;

		LCD.clear();
		LCD.drawString("Press ENTER", 0, 0);
		LCD.drawString("to callibrate", 0, 1);
		LCD.drawString(color, 0, 2);
		while (!Button.ENTER.isPressed()) {
			lightValue = ls.readValue();
			LCD.drawInt(lightValue, 4, 10, 2);
			LCD.refresh();
		}
		return lightValue;
	}

	private int readRaw(String color) {
		int lightValue = 0;
		

		while (Button.ENTER.isDown())
			;
		
		LCD.clear();
		LCD.drawString("Press ENTER", 0, 0);
		LCD.drawString("to callibrate", 0, 1);
		LCD.drawString(color, 0, 2);
		while (!Button.ENTER.isPressed()) {
			lightValue = port.readRawValue();
			LCD.drawInt(lightValue, 4, 10, 2);
			LCD.refresh();
		}
		return lightValue;
	}
	
	public void calibrate() {
		blackLightValue = readRaw("black");
		whiteLightValue = readRaw("white");
		thirdColorValue = readRaw(thirdColorName);
		// The threshold is calculated as the median between
		// the two readings over the two types of surfaces
		
		upperThreshold = ((blackLightValue - thirdColorValue) / 7) + thirdColorValue;
		lowerThreshold = (thirdColorValue + whiteLightValue) / 2;
		
		//TETSTETETTSTET
	     LCD.drawString("bg 1: ", 0, 3); 
	     LCD.drawString("wg 2: ", 0, 4); 
	     
	     LCD.drawInt(upperThreshold, 5, 3);
	     LCD.drawInt(lowerThreshold, 5, 4);
	     try {
			Thread.sleep(4000);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}

	public boolean black() {
		return (port.readRawValue() > upperThreshold);
	}

	public boolean white() {
		return (port.readRawValue() < lowerThreshold);
	}
	
	public int getBlack(){
		return blackLightValue;
	}
	
	public boolean isThirdColor() {
		int reading = port.readRawValue();
		return (upperThreshold > reading && reading > lowerThreshold);
	}
	
	public boolean isThirdColor(int color) {
		return (upperThreshold > color && color > lowerThreshold);
	}

	public int light() {
		return port.readRawValue();
	}

	public int getOffset()
	{
		return 497; //lowerThreshold;
	}
	
	public int getLower()
	{
		return lowerThreshold;
	}
	
	public int getUpper()
	{
		return upperThreshold;
	}
}